//
// Created by 32827 on 2023/12/2.
//
#include "yzl_i2c_mpu6050.h"
#include "yz_i2c.h"

YZF_BOOL yzl_mpu6050_check(YZF_I2C_POINTER i2c){
    uint8_t data=0;
    yz_i2c_add7_mem_receive(i2c,YZL_MPU6050_ADDRESS, YZL_MPU6050_WHOIS,&data,1);
    return data==0x68 ? YZF_TRUE:YZF_FALSE;
}
YZF_RESULT yzl_mpu6050_init(YZF_I2C_POINTER i2c){
    //复位
    delay_ms(500);
    uint8_t data=0x00;
    yz_i2c_add7_mem_transmit(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_POWER1_ADDRESS,&data,1);
    delay_ms(500);
    data=0x01;
    yz_i2c_add7_mem_transmit(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_POWER1_ADDRESS,&data,1);
    delay_ms(5);
    data=0x07;
    yz_i2c_add7_mem_transmit(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_SIMPLE_RATE_ADDRESS,&data,1);
    delay_ms(5);
    data=0x06;
    yz_i2c_add7_mem_transmit(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_DLPF_ADDRESS,&data,1);
    delay_ms(5);
    data=0x01;
    yz_i2c_add7_mem_transmit(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_ACCELEROMETER_ADDRESS,&data,1);
    delay_ms(5);
    data=0x10;
    yz_i2c_add7_mem_transmit(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_GYROSCOPE_ADDRESS,&data,1);
    delay_ms(200);
    return YZF_OK;
}
YZF_RESULT yzl_mpu6050_read_acc(YZF_I2C_POINTER i2c,uint16_t *data){
    yz_i2c_add7_mem_receive(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_ACCELEROMETER_X_ADDRESS,(uint8_t *)data,6);
    uint8_t *p=(uint8_t *)data;
    uint8_t temp;
    //高低位交换
    temp=p[0];
    p[0]=p[1];
    p[1]=temp;
    temp=p[2];
    p[2]=p[3];
    p[3]=temp;
    temp=p[4];
    p[4]=p[5];
    p[5]=temp;
    return YZF_OK;
}
YZF_RESULT yzl_mpu6050_read_gyro(YZF_I2C_POINTER i2c,uint16_t *data){
    yz_i2c_add7_mem_receive(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_GYROSCOPE_X_ADDRESS,(uint8_t *)data,6);
    uint8_t *p=(uint8_t *)data;
    uint8_t temp;
    //高低位交换
    temp=p[0];
    p[0]=p[1];
    p[1]=temp;
    temp=p[2];
    p[2]=p[3];
    p[3]=temp;
    temp=p[4];
    p[4]=p[5];
    p[5]=temp;
    return YZF_OK;
}
YZF_RESULT yzl_mpu6050_read_temperature(YZF_I2C_POINTER i2c,uint16_t *data){
    yz_i2c_add7_mem_receive(i2c,YZL_MPU6050_ADDRESS,YZL_MPU6050_REG_TEMPERATURE_ADDRESS,(uint8_t *)data,2);
    uint8_t *p=(uint8_t *)data;
    uint8_t temp;
    //高低位交换
    temp=p[0];
    p[0]=p[1];
    p[1]=temp;
    return YZF_OK;
}
#if USE_SOFT_PORT
YZF_BOOL yzl_mpu6050_soft_check(YZL_Soft_I2C* i2c){
    uint8_t data=0;
    uint8_t param=YZL_MPU6050_WHOIS;
    YZF_RESULT re=yzl_softport_i2c_read(i2c,YZL_MPU6050_ADDRESS, &param,1,&data,1);
    return data==0x68 ? YZF_TRUE:YZF_FALSE;
}
YZF_RESULT yzl_mpu6050_soft_init(YZL_Soft_I2C* i2c){
    //复位
    uint8_t data=0x80;
    uint8_t param=YZL_MPU6050_REG_POWER1_ADDRESS;
    YZF_RESULT re;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(200);
    data=0x01;
    param=YZL_MPU6050_REG_POWER1_ADDRESS;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(200);
    data=0x00;
    param=YZL_MPU6050_REG_POWER2_ADDRESS;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(5);
    data=0x00;
    param=YZL_MPU6050_REG_INT_ADDRESS;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(5);
    data=0x00;
    param=YZL_MPU6050_REG_SIMPLE_RATE_ADDRESS;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(5);
    data=0x04;
    param=YZL_MPU6050_REG_DLPF_ADDRESS;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(5);
    data=0b00000000;//+- 16g
    param=YZL_MPU6050_REG_ACCELEROMETER_ADDRESS;
    re=yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(5);
    data=0b00000000;//+- 2000
    param=YZL_MPU6050_REG_GYROSCOPE_ADDRESS;
    re= yzl_softport_i2c_param_write(i2c,YZL_MPU6050_ADDRESS,&param,1,&data,1);
    delay_ms(5);
    return YZF_OK;
}
YZF_RESULT yzl_mpu6050_soft_read_acc(YZL_Soft_I2C* i2c,uint16_t *data){
    uint8_t param=YZL_MPU6050_REG_ACCELEROMETER_X_ADDRESS;
    yzl_softport_i2c_read(i2c,YZL_MPU6050_ADDRESS,&param,1,(uint8_t *)data,6);
    uint8_t *p=(uint8_t *)data;
    uint8_t temp;
    //高低位交换
    temp=p[0];
    p[0]=p[1];
    p[1]=temp;
    temp=p[2];
    p[2]=p[3];
    p[3]=temp;
    temp=p[4];
    p[4]=p[5];
    p[5]=temp;
    return YZF_OK;
}
YZF_RESULT yzl_mpu6050_soft_read_gyro(YZL_Soft_I2C* i2c,uint16_t *data){
    uint8_t param=YZL_MPU6050_REG_GYROSCOPE_X_ADDRESS;
    yzl_softport_i2c_read(i2c,YZL_MPU6050_ADDRESS,&param,1,(uint8_t *)data,6);
    uint8_t *p=(uint8_t *)data;
    uint8_t temp;
    //高低位交换
    temp=p[0];
    p[0]=p[1];
    p[1]=temp;
    temp=p[2];
    p[2]=p[3];
    p[3]=temp;
    temp=p[4];
    p[4]=p[5];
    p[5]=temp;
    return YZF_OK;
}
YZF_RESULT yzl_mpu6050_soft_read_temperature(YZL_Soft_I2C* i2c,uint16_t *data){
    uint8_t param=YZL_MPU6050_REG_TEMPERATURE_ADDRESS;
    yzl_softport_i2c_read(i2c,YZL_MPU6050_ADDRESS,&param,1,(uint8_t *)data,2);
    uint8_t *p=(uint8_t *)data;
    uint8_t temp;
    //高低位交换
    temp=p[0];
    p[0]=p[1];
    p[1]=temp;
    return YZF_OK;
}
#endif